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(57) ABSTRACT 

A robotic system includes a robot having manipulators for 
grasping an object using one of a plurality of grasp types 
during a primary task, and a controller. The controller con- 
trols the manipulators during the primary task using a mul- 
tiple-task control hierarchy, and automatically parameterizes 
the internal forces of the system for each grasp type in 
response to an input signal. The primary task is defined at an 
object-level of control, e.g., using a closed-chain transforma- 
tion, such that only select degrees of freedom are commanded 
for the object. A control system for the robotic system has a 
host machine and algorithm for controlling the manipulators 
using the above hierarchy. A method for controlling the sys- 
tem includes receiving and processing the input signal using 
the host machine, including defining the primary task at the 
object-level of control, e.g., using a closed-chain definition, 
and parameterizing the internal forces for each of grasp type. 
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HIERARCHICAL ROBOT CONTROL 
SYSTEM AND METHOD FOR 
CONTROLLING SELECT DEGREES OF 
FREEDOM OF AN OBJECT USING 

MULTIPLE MANIPULATORS 5 

CROSS-REFERENCE TO RELATED 
APPLICATIONS 

The present application claims the benefit of and priority to 
U.S. Provisional Application No. 61/174,316 filedonApr. 30, 
2009. 

STATEMENT REGARDING FEDERALLY 
SPONSORED RESEARCH OR DEVELOPMENT 

15 

This invention was made with government support under 
NASA Space Act Agreement number SAA-AT-07-003. The 
government may have certain rights in the invention. 

TECHNICAL FIELD 20 

The present invention relates to a system and method for 
controlling one or more humanoid robots having a plurality of 
joints and multiple degrees of freedom. 

25 

BACKGROUND OF THE INVENTION 

Robots are automated devices that are able to manipulate 
objects using manipulators, e.g., hands, fingers, thumbs, etc., 
and a series of links interconnected via robotic joints. Each 30 
joint in a typical robot represents at least one independent 
control variable, i.e., a degree of freedom (DOF). End-effec- 
tors or manipulators are used to perform the particular task at 
hand, e.g., grasping a work tool or other object. Therefore, 
precise motion control of the robot may be organized by the 35 
level of task specification; object-level control, which 
describes the ability to control the behavior of an object 
grasped or held in either a single or a cooperative grasp of a 
robot, end-effector control, and joint-level control. Collec- 
tively, the various control levels achieve the required robotic 40 
mobility, dexterity, and work task-related functionality. 

Humanoid robots are a particular type of robot having an 
approximately human structure or appearance, whether a full 
body, a torso, and/or an appendage, with the structural com- 
plexity of the humanoid robot being largely dependent upon 45 
the nature of the work task being performed. The use of 
humanoid robots may be preferred where direct interaction is 
required with devices or systems that are specifically made 
for human use. The use of humanoid robots may also be 
preferred where interaction is required with humans, as the 50 
motion can be programmed to approximate human motion 
such that the task queues are understood by the cooperative 
human partner. 

Due to the wide spectrum of work tasks that may be 
expected of a humanoid robot, different control modes may 55 
be simultaneously required. For example, precise control 
must be applied within the different control spaces noted 
above, as well as control over the applied torque or force of a 
given motor-driven joint, joint motion, and/or the various 
grasp types. Deploying humanoid robots in assembly line 60 
tasks requires an ability to interact with unstructured environ- 
ments and to implement diverse applications. 

SUMMARY OF THE INVENTION 

65 

Accordingly, a robotic control system and method are pro- 
vided herein for controlling a robot or multiple robots via a 


2 

control framework as set forth below. Complex control over a 
robot, e.g., a humanoid robot having multiple DOF, such as 
over 42 DOF in one particular embodiment, may be provided 
over the many independently-moveable and interdepen- 
dently-moveable robotic joints and object end-effectors or 
manipulators, or of manipulators of more than one robot 
simultaneously applying a cooperative grasp on an object. 
The framework disclosed herein is based on multiple-priority 
tasks, and therefore is hierarchical in nature. The primary task 
is defined at the object-level of control, e.g., using a “closed- 
chain” Jacobian transformation and/or a “closed chain” grasp 
matrix as explained in detail herein. This provides for a task 
that commands only select degrees of freedom (DOF) for the 
object, allowing the other DOF to remain free or uncon- 
strained. This in turn creates an integrated null space that 
includes not only the redundant DOF of each individual 
robotic manipulator, e.g., a hand, multiple fingers/thumb, 
etc., but also the free DOF of the object shared across the 
various manipulators. The secondary task, on the other hand, 
may be defined at the joint-level of control, i.e., in the joint 
space. This multiple-priority control framework offers great 
functionality for cooperative assembly applications, particu- 
larly using a highly complex humanoid robot of the type 
described herein. 

Within the scope of the invention, the controller provides 
automatic parameterization of internal forces during multiple 
robot grasp types. Such grasp types may include, by way of 
example, a cooperative two-hand grasp and a cooperative 
three-finger grasp of an object. Both possibilities are 
described in mathematical detail herein. 

In particular, a robotic system is provided herein that 
includes one or more manipulators, whether of a single robot 
ormultiple robots, collectively adapted for grasping an object 
using one of a plurality of grasp types during an execution of 
a primary task, and a controller. The controller is electrically 
connected to the robot(s), and controls the manipulator(s) 
during execution of the primary task using a multiple-task 
control hierarchy. The controller automatically parameterizes 
internal forces of the robotic system for each of the grasp 
types in response to the input signal, with the primary task 
being defined at an object-level of control, e.g., using a 
closed-chain motion transformation in one embodiment. 

A controller is also provided for the robotic system noted 
above. The controller includes a host machine electrically 
connected to the robot(s), and an algorithm executable by the 
host machine. The algorithm is adapted to control, when 
executed, the plurality of manipulators using a multiple-task 
control hierarchy. Execution of the algorithm automatically 
parameterizes internal forces of the robotic system for each of 
a plurality of grasp types of the robot(s), with the primary task 
being defined at an object-level. 

A method for controlling the robotic system as described 
above includes receiving the input signal via the host 
machine, and processing the input signal via a multiple-task 
control hierarchy, using the host machine, to thereby control 
the plurality of manipulators during the execution of the 
primary task. Processing the input signal includes: defining 
the primary task at the object-level of control, and automati- 
cally parameterizing internal forces of the robotic system for 
each of the plurality of grasp types in response to the input 
signal. 

The above features and advantages and other features and 
advantages of the present invention are readily apparent from 
the following detailed description of the best modes for car- 
rying out the invention when taken in connection with the 
accompanying drawings. 
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BRIEF DESCRIPTION OF THE DRAWINGS 

FIG. 1 is a schematic illustration of a robotic system having 
a robot that is controllable using a hierarchical, multiple-task 
control framework in accordance with the invention; and 

FIG. 2 is a schematic illustration of the various forces and 
coordinates related to an object that may be grasped by a 
robot, such as of the type shown in FIG. 1. 

DESCRIPTION OF THE PREFERRED 
EMBODIMENT 

With reference to the drawings, wherein like reference 
numbers refer to the same or similar components throughout 
the several views, and beginning with FIG. 1 , a robotic system 
11 is shown having a robot 10, e.g., a dexterous humanoid- 
type robot, that is controlled via a control system or controller 
(C) 22. While one robot 10 is shown, the system 11 may 
include more than one robot as set forth below. The controller 
22 is electrically comiected to the robot 10, and is adapted for 
controlling the various end-effectors or object manipulators 
of the robot 10, as described below, using algorithm(s) 100 
suitable for implementing a multiple-task control hierarchy. 
In this control hierarchy, an impedance relationship may 
operate, in some embodiments, in a null space at the object- 
level of control, although the hierarchy is not limited to 
impedance control. The controller 22 automatically param- 
eterizes internal forces of the system 11 for multiple grasp 
types of the robot 10 in response to input signals (arrow i c ) to 
the controller 22, and/or generated by or external to the con- 
troller. A closed-chain Jacobian motion transformation or 
task definition, also as described below, may be used to define 
a primary task of the robot 10 at the object-level of control in 
one embodiment. 

The robot 10 is adapted to perform one or more automated 
tasks with multiple degrees of freedom (DOF), and to per- 
form other interactive tasks or control other integrated system 
components, e.g., clamping, lighting, relays, etc. According 
to one embodiment, the robot 10 is configured as a humanoid 
robot as shown, with over 42 DOF being possible in one 
embodiment. The robot 10 has a plurality of independently 
and interdependently-moveable manipulators, e.g., hands 18, 
fingers 19, thumbs 21, etc., and including various robotic 
joints. The joints may include, but are not necessarily limited 
to, a shoulder joint, the position of which is generally indi- 
cated by arrow A, an elbow joint (arrow B), a wrist joint 
(arrow C), a neck joint (arrow D), and a waist joint (arrow E), 
as well as the finger joints (arrow F) between the phalanges of 
each robotic finger. 

Each robotic joint may have one or more DOF. For 
example, certain compliant joints such as the shoulder joint 
(arrow A) and the elbow joint (arrow B)may have at least two 
DOF in the form of pitch and roll. Likewise, the neck joint 
(arrow D) may have at least three DOF, while the waist and 
wrist (arrows E and C, respectively) may have one or more 
DOF. Depending on task complexity, the robot 10 may move 
with over 42 DOF, as noted above. Each robotic joint may 
contain and may be internally driven by one or more actua- 
tors, e.g., joint motors, linear actuators, rotary actuators, and 
the like. 

The robot 10 may include human-like components such as 
a head 12, torso 14, waist 15, and amis 16, as well as certain 
manipulators, i.e., hands 18, fingers 19, and thumbs 21, with 
the various joints noted above being disposed within or 
between these components. The robot 10 may also include a 
task-suitable fixture or base (not shown) such as legs, treads, 
or another moveable or fixed base depending on the particular 
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application or intended use of the robot. A power supply 13 
may be integrally mounted to the robot 10, e.g., a recharge- 
able battery pack carried or worn on the back of the torso 14 
or another suitable energy supply, or which may be attached 
5 remotely through a tethering cable, to provide sufficient elec- 
trical energy to the various joints for movement of the same. 

The controller 22 provides precise motion control of the 
robot 10, including control overthe fine and gross movements 
needed for manipulating an object 20 via the manipulators 
to noted above. That is, object 20 may be grasped by the fingers 
19 and thumb 21 of one or more hands 18. The controller 22 
is able to independently control each robotic joint and other 
integrated system components in isolation from the other 
joints and system components, as well as to interdependently 
15 control a number of the joints to fully coordinate the actions 
of the multiple joints in performing a relatively complex work 
task. 

Still referring to FIG. 1, the controller 22 may include 
multiple digital computers or data processing devices each 
20 having one or more microprocessors or central processing 
units (CPU), read only memory (ROM), random access 
memory (RAM), erasable electrically-programmable read 
only memory (EEPROM), a high-speed clock, analog-to- 
digital (A/D) circuitry, digital -to-analog (D/A) circuitry, and 
25 any required input/output (I/O) circuitry and devices, as well 
as signal conditioning and buffer electronics. Individual con- 
trol algorithms resident in the controller 22 or readily acces- 
sible thereby may be stored in ROM and automatically 
executed at one or more different control levels to provide the 
30 respective control functionality. 

The controller 22 may include a server or host machine 17 
configured as a distributed or a central control module, and 
having such control modules and capabilities as might be 
necessary to execute all required control functionality of the 
35 robot 10 in the desired manner. Additionally, the controller 22 
may be configured as a general purpose digital computer 
generally comprising a microprocessor or central processing 
unit, read only memory (ROM), random access memory 
(RAM), electrically-erasable programmable read only 
40 memory (EEPROM), a high speed clock, analog-to-digital 
(A/D) and digital-to-analog (D/A) circuitry, and input/output 
circuitry and devices (I/O), as well as appropriate signal con- 
ditioning and buffer circuitry. Any algorithms resident in the 
controller 22 or accessible thereby, including an algorithm 
45 100 for executing the hierarchical, impedance-based control 
framework described in detail below, may be stored in ROM 
and executed to provide the respective functionality. 

The controller 22 may be electrically connected to a 
graphical user interface (GUI) 24 providing intuitive access 
50 to the controller. GUI 24 could provide an operator or pro- 
grammer with control access to a wide spectrum of primary 
and secondary work tasks, i.e., the ability to control motion in 
the object-level, end effector-level, and/or joint space-level or 
levels of the robot 10. The GUI 24 may be simplified and 
55 intuitive, allowing a user, through simple graphical or icon- 
driven inputs, to control the robot 10 by inputting an input 
signal (arrow i c ), e.g., a desired force or torque imparted to 
the object 20 by one or more of the aforementioned manipu- 
lators, or a desired action of the robot. 

60 In order to perform a range of manipulation tasks using the 
robot 10, or multiple robots, a wide range of functional con- 
trol over the robot(s) is required. This functionality includes 
hybrid force/position control, object-level control with 
diverse cooperative grasp types, end-effector Cartesian space 
65 control, i.e., control in the XYZ Cartesian coordinate space, 
and joint space manipulator control, and with a hierarchical 
prioritization of the multiple control tasks. The invention 
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provides for a parameterized space of internal forces to con- 
trol such a cooperative grasp. It also provides, in one embodi- 
ment, for a secondary joint space impedance relation that 
operates in the null-space of the object 20, as explained math- 
ematically below. 5 

Impedance Law: 

The first step of the control framework set forth herein is to 
characterize the dynamic behavior of the object 20 being 
acted on by the robot 10, or by two or more robots grasping 
the same object. This section presents the closed-loop dynam- to 
ics, with the passive dynamics described later herein. The 
desired closed-loop behavior may be defined by the following 
impedance relationship, i.e., equation (1): 


M„y + B 0 y + K a Ay = F - F* 



In this formula, M 0 , B 0 , and K 0 are the commanded inertia, 
damping, and stiffness matrices respectively, where all eR 6 * 6 . 
v is the linear velocity of the object 20 center of mass, while 
w is the angular velocity of the obj ect. Both are measured with 
respect to the ground reference frame. F and F* represent the 25 
net actual and desired external wrench on the object. Ay is the 
position error (y-y*). Without loss of generality, the orienta- 
tion component of y is expressed through an angle-axis rep- 
resentation, as shown in equation (12) below. At equilibrium, 
where y=y=0, the impedance relation specifies that the inter- 30 
nal force F should be the sum of the nominal force F* and the 
spring force K 0 Ay. If it is desired for some directions to be 
pure force control, this may be accomplished by setting the 
stiffness of those directions to zero in K 0 . Setting some direc- 
tions to a pure force control and setting the complementary' 
components of F* to zero, one has a “hybrid” scheme of force 35 
and motion control in orthogonal directions. 

The redundancy of the manipulators allows for a secondary 
task to act in the null space of the object impedance. For the 
sake of this secondary task, a joint space impedance law is 
defined as follows in equation (2): 40 

MjCj+Bjq+KjAq=T : f 

In equation (2) above, M,, B ■ and here are the commanded 
inertia, damping, and stiffness matrices, respectively, for the 
joint space, q is the column matrix of joint angles for all 45 
manipulators in the system. Aq is the joint position error. x f 
represents the column matrix of joint torques produced by 
forces acting on the manipulator. These two impedance laws 
result in the following task objectives for the controller: 

y*=M~' ( F-F*-BJ-K 0 Ay ) 

*=Mf l (Tj-Bjtj-KjAq) 

i.e., equations (3), where y* is the desired object acceleration, 
and q*,„ the desired joint acceleration for the null space (ns). 55 

Open-Chain Kinematics: 

Referring to FIG. 2, the free-body diagram 25 of the object 
20 and the coordinate system are shown, with N and B rep- 
resenting the ground and body reference frames, respectively. 
r ( is the position vector from the center of mass to contact 60 
point i, where i=l, . . . , n. f, and t, represent the contact force 
and moment, respectively, from point i. Hie standard kine- 
matic relationship may be defined for rigid-body accelera- 
tions as: 

v i -=v+toxr r KOx((t>xr i -)+ 2<$-xx reli +a,. eU 65 


co^oo+a^i.e., 


equations (4). 


v reIi and a. reh are defined as the first and second derivatives, 
respectively, of r, in the object frame, as shown in equations 
(5): 


V„ii ± B — r i , 
at 


a reti ~B Vreti- 

dr 


These relations can be expressed in matrix form as the famil- 
iar grasp mapping. Let x represent the column matrix of 
end-effector velocities that are constrained by the contact; the 
exact form of that will follow shortly. Given this definition, 
the mapping for accelerations follows as equation (6): 

x=Gy+h 

G is known as the grasp matrix, providing the mapping forthe 
contact information, h is a column matrix of centripetal, 
coriolus, and relative accelerations. The forms of G and h 
depend on the grasp type, as discussed below. To map x down 
to the manipulator space, the following Jacobian matrices are 
introduced. The linear and rotational Jacobians, J V! . and J MI 
respectively, are defined as follows in equations (7): 

Stacking these submatrices into a composite Jacobian J, 
where x=Jq, the grasp map of equation (6) can be expressed as 
the following transformation, equation (8), between joint and 
object accelerations: 

Jq+jq=Gy+h 

Grasp Types: 

In this transformation, the structures of J, G, and h depend 
on the grasp type. To illustrate, we will consider two grasp 
types: a Two Hand Grasp, and a Three Finger Grasp. A hand 
grasp represents a rigid contact that can transfer both arbitrary 
forces and moments. It thus constrains both the linear and 
angular motion of the end-effector. The finger contact repre- 
sents a no-slip, point contact that can only transfer forces. It 
thus constrains only the linear motion of the end-effector. 
Accordingly, the matrices take on the following form for each 
type. 

Two Hand Grasp: 



' Vi 


■Ad 1 


h 

-4‘ 


' j-i - 


COi 


JojI 


0 

h 


&reli 

X = 

V2 

, J = 

Jv 2 

, G = 

h 

~ r 2 

,h = 



, 


Juj2 \ 


_ 0 

h . 


k ®rel2 , 


Three Finger Grasp: 





^vl j 


h -d 


f Al ' 

X = 

v 2 

, J = 

J\2 

, G = 

h -d 

, h = 

^2 


k V 3 > 


. Jv3 \ 


h -d. 


k^-3 , 


In these equations, L ! =wx(wxr ! )+2mxv reZ! +a re ,, I . In practice, 
the relative velocities will be considered negligible and the 
relative accelerations will consist of closed-loop servos to 
regulate the internal forces. \ k represents the kxk identity 
matrix, and r* represents the skew-symmetric matrix equiva- 
lent for the cross product of r, or: 
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Closed-Chain Kinematics: 

The next step of the present control framework is to map 
the endpoint DOF down to the manipulator space. For this 
purpose, we introduce the closed-chain Jacobian. This trans- 10 
formation defines a task that commands only select DOF of 
the object. The uncommanded DOF are incorporated into the 
null-space of the primary task. This allows the secondary task 
to be optimized in a space that includes not only the redundant 
DOF of each individual manipulator of the robot 10, but also 15 
the free DOF of the object shared across the manipulators. It 
also allows the primary task to operate in an expanded work- 
space. This may provide a considerable control advantage 
since the object 20 is now limited to the union of multiple 
workspaces. 20 

To derive this closed-chain Jacobian, consider the motion 
constraints between the end-effectors and the object 20. 
These motion or holonomic constraints provide the link 
between the object DOF and the manipulator DOF. In a point 
contact, these constraints apply to position only, akin to a 25 
spherical joint. In a rigid contact, the same constraints apply 
to all six DOF of the end-effector, assuming that no slip 
occurs. Given the full set of motion constraints, one may then 
explicitly eliminate the uncommanded DOF of the object 20 
to solve for the reduced, independent set of motion con- 30 
straints. This technique produces relatively simple results that 
require no extra real-time computation to derive. 

Let z represent the p DOF of the object to be commanded 
by the primary task. To this end, one may introduce a constant 
px6 matrix S which picks out the directions to control. The 35 
relation between the full and reduced sets of DOF, as well as 
its inverse, follows: 


z=Sy 

no 

y=S + z+S ± \i 

(12) 


Here, S + is the pseudoinverse of S, S 4 is a 6x(6-p) matrix 
spanning the null space of S, and pe R 6 ~ p is arbitrary. The 
transformation in equation (8) represents the full set of 
motion constraints between the object and the end-effectors 42 
or manipulators, and these constraints contain free param- 
eters. To reduce this set to a minimum set of constraints, the 
free parameters p may be eliminated to shift the free param- 
eters to the null space of the task, where they become avail- 
able to the secondary task of the robot 10. 

Substituting equation (12) into equation (8) derives equa- 
tion (13): 

Jq+jq=G(S*z+S^\i)+h (13) 

To eliminate p, find a full-rank matrix E such that EGS ±= 0, 55 

i.e., equation (14), where 

Ee R (6n+p— 6)x6n 

Ee. 

Multiplying equation (13) by E provides the reduced set: 60 
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Matrix EJ plays a similar role in closed-chain kinematics as 
the Jacobian matrix usually plays in open-chain kinematics. 
Therefore, the following matrices may be derived: 

i=EJ, J =EJ, G=EGS*S, h=Eh. (16) 

This allows for the definition of a final closed-chain transfor- 
mation: 

Jq + J q =Gy+h (17) 

J and G are defined as the “closed chain” Jacobian and grasp 
matrix, respectively. 

Consider three task types: 

1 . Full pose control, where: S=I 6 , S + =I 6 , S x =0. 

2. Orientation-only control, where: 


r o 1 r / 3 1 
,= [0 /3l^ = [ /3 J.r = [ 0 J. 

3. Position-only control, where: 


Co 

II 

' 0 ' 

L o j 

h.' 


Two-Hand Grasp: 

Full Pose: 

Since this scenario involves no reduction in DOF, the 
closed-chain expressions remain unchanged, and: 

j=J, G=G, h=h (21) 

Orientation Only: 

The following matrix is a valid annihilator for this sce- 
nario: 


£ = 


h 0 -/ 3 0 
0 / 3 0 0 
ooo h 


Given this E, the closed-chain definitions of equations (16) 
result in the following matrices for the orientation-only con- 
trol of a two hand grasp: 



Jvl - J\2 


X *T 

l 

o 


r Ai - A 2 ' 

J = 

Joj 1 

, G = 

0 / 3 

,h = 

&relj 


J <o2 


0 /3 


K &rel 2 , 


Throughout these scenarios, the form for J follows J directly, 
where the Jacobian submatrices are simply replaced with 
their derivatives. 

Position Only: 

The following matrix is a valid annihilator for this sce- 
nario: 


EJq + EJq = EGS*Z + Eh 
= EGS^Sy + Eh 


( 15 ) 


65 


I, r( 0 0 

0 0 /3 ^2 

0 /3 0 —1^ 
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Given this E, the closed-chain definitions of equations (16) 
result in the following matrices for the position-only control 
of a two hand grasp: 


5 


F ma = F + G T f 4- mg 


Fma 


mac 

Igco + wx IqO) + rc x mac 



(25) 



J\ 1 + J^i 


h 

0‘ 


" Ai + rf a rell ' 

J = 

J\2 + r 2 J(o2 

, G = 

h 

0 

, h = 

A 2 + r2 a rel 2 


. - Joj2 . 


. 0 

0. 


, ®rel j O- rel 2 , 


Three-Finger Grasp: 

In a three-finger grasp scenario, one is dealing with point 
contacts, and the motion constraints apply only to the position 
of the endpoints. 

Full Pose: 

Since this scenario involves no reduction in DOF, the 
closed-chain expressions remain unchanged, and: 

J=J, G=G, h=h (21) 

Orientation Only: 

The following matrix is a valid annihilator for this sce- 
nario: 


E = 


1 3 —1 2, 0 

h 0 -h . 


Given this E, the closed-chain definitions of equations (16) 
result in the following matrices for the orientation-only con- 
trol of a three finger grasp: 


J = 


Jvl 

J v l 



0 'U41 j i _Gh-A 2 'j 
0 r\ -r\ ' l Ai -A 3 J 


( 22 ) 


F ' ma represents the inertial forces, where m is the mass of the 
object 20 and l G is the moment of inertia about the center of 
10 mass G. a G is the acceleration of G, and r G is the position 
vector from the reference point to G. f is the column matrix of 
contact wrenches; its form mirrors the form of x shown in 
equations (9) and (10) above, g is the gravity vector. 

Internal Forces: 

15 As seen in this equation of motion, the contact forces are 
mapped to the object space through the transpose of the grip 
matrix. Accordingly, the internal forces on the object 20 are 
defined by the null space of G r . To apply the control of 
internal forces, two qualifies are desired. First, the null space 
_, 0 should be parameterized with physically relevant parameters . 
Second, the parameters should lie in the null space of both 
grasp types. These requirements are satisfied by the concept 
of interaction forces. Drawing a line between two contact 
points, the interaction force is the difference between the two 
, - contact forces projected along that line, as is known in the art. 
Thus, one can parameterize the internal forces of system 10 
with the various interaction components. 

As described earlier, one may use the relative acceleration 
terms to control the internal forces. To ensure that these 
30 relative accelerations affect only the internal forces and not 
the external dynamics, they too must lie in the null space of 
G T . If x rel is the column matrix of relative accelerations, that 
condition is met when: x re ^K(G r ). Accordingly, we use the 
relative accelerations to close a servo loop about the interac- 
35 tion forces. Defining u y as the unit vector pointing from con- 
tact i to j, the magnitude of the interaction force, 1 L, between 
those two contacts follows. 


Position Only: 40 fj = ( f - fj) • « tf 

This scenario is more challenging, given the difficulty of . rj - ri 

explicitly eliminating the free variable to from the set of Uij = ||r, - 

motion constraints. For this scenario: 


(26) 


GS" = 


-4 

~ r 2 


~>3 


r 3 = ar t + fSr 2 + yn xr 2 . 


4S We will introduce the interaction acceleration, a tJ , as a PI 
(23) regulator on these forces, where \a P and k 7 are constant gains . 

fl#=W(rA)-W(rA)* ( 27 ) 

Noting that u,y -u yl and a, 7 = a /I , the internal acceleration for 
three contacts can be summarized as follows. For the two 
contact case, simply set a 13 =0. 


Where a, |3, and y are scalars to be solved for in equation (23). 

E may then be derived as: 

55 


a rell =a 12 U l2 +a 13^13 
a rel2 = ~ a I2 u l2 +a 23 u 23 
a rel3 = ~ a I3 u l3~ a 23 u 23 


(28) 


r[ 0 0 

0 r\ 0 

rl r\ 0 

0-/3 — yr\ Ph+yr\ -/ 3 


(24) 


Equation of Motion: 

Consider again the free-body diagram of FIG. 2. f ; and t, 
represent the contact force and moment, respectively, from 
contact i. The equation of motion can be expressed as: 


Since we choose not to control any rotational component, 
a re2 ,=0 for all i. 

Control Law: 

60 Using these impedance tasks, motion transformations, and 
internal forces, the control law may be presented. First, start 
by modeling the equations of motion for the full system of 
manipulators: 

65 Mq+c-xj^t. (29) 

M is the joint space inertia matrix, c is the column matrix of 
Coriolus, centripetal and gravitational generalized forces, 
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and x is the column matrix of joint torques. Assuming that 
forces act on the manipulator only at the end-effector, 

x r -J T f. (30) 

In preparation for the control law, some unsensed quanti- 
ties for the object 20 are estimated. First, the external wrench 
(F) is estimated from the other forces on the object 20 . Refer- 
ring to equation (25), one may employ a quasi-static approxi- 
mation of the forces. 

F~~G r f-mg (31) 

Although included here, the object weight can also be 
neglected in most cases . In addition, the object velocity can be 
estimated with the following least-squares error estimate of 
the system as a rigid body: 

y=G*Jq, (32) 

where the superscript (+) indicates the pseudoinverse of the 
respective matrix. 

Finally, we present the control law based on the following 
Inverse Dynamics formulation [12]. 

x=Mij*+c-Xf (33) 

q* in this expression is the commanded joint acceleration. It 
can be derived from the commanded object acceleration, y*, 
according to equation (17). 

q*=>($y , +h-Jq)+Nyq„’ 

Nj=I-j+j (34) 

Nj denotes the orthogonal projection operator for the null 
space of J and q* MiS is the vector of accelerations projected into 
that null space. Using this closed-chain Jacobian, the second 
task will thus be optimized in a space that includes the free 
DOF of the object. The two commanded accelerations, y * and 
q*,„, are found from the impedance tasks in equation (3). 

The explicit control law can be spelled out from equations 
(33), (34) and (3). Introducing the force estimates in equa- 
tions (30) and (31), the final control law follows as equation 
(35): 


t = -MJ + GM~ l (F* + B 0 y + K 0 Ay + G T f + mg') + 

MJ*\h - 7?j - MN } Mj l ( Bjq + KjAq + J T f) + c + J T f 

To understand the true behavior of the system, consider the 
following closed-loop analysis. By noting that Nj 2 =Ny and 
NjJ + =0, we obtain the following independent closed-loop 
dynamics for both the range space and null space of the 
system. 

S[y+M 0 ~\BJ+K < £y-AF)\=S[M- l F ma ] (36) 

Ntfq+Mr'iBjq+KjAq-Xj)]^ (37) 

The first relation reveals the desired object impedance task 
applied to the DOF selected by S. If the impedance matrices 
are diagonal, the task spaces will remain decoupled. The 
right-hand side of this relation represents a disturbance from 
the object accelerations due to the quasi-static estimation of F. 
This disturbance does not affect the internal forces. The sec- 
ond relation shows that the desired second impedance task is 
implemented with a minim um -error projection into the null 
space. 

This control law was able to eliminate the need for the 
object dynamics through two features. First, it introduced the 
feedback on the end-effector forces. Second, it conducted the 


12 

transformation from the object space to the end-effector space 
using accelerations, rather than forces. This method will 
maintain the internal forces with greater integrity than other 
control laws that rely on estimates of the object inertia and 
5 acceleration. Although the external dynamics will witness the 
aforementioned disturbance, in our opinion, the infernal 
forces are the critical factor in cooperative manipulation. 

Zero Force Feedback: 

Unfortunately, force sensing will not always be available 
10 on each end-effector. This section will thus introduce a ver- 
sion of the control law that eliminates the need for the force 
feedback. The solution presented here, however, does not 
have the full range of capabilities. It is only applicable to a 
scenario with full-pose control applied to the Two Hand 
! . Grasp. The force feedback terms in the control law (35) can be 
eliminated by the appropriate selection of the active inertias, 
M 0 and M . The feedback is eliminated when the coefficients 
of f sum to zero: 

J t -M^GM-'G t -MN,Mj- 1 J t =0. (38) 

20 Solving for this relation results in the following two condi- 
tions: 

M^‘=G*(7Ar‘/ r )G r * (39) 

M:=M (40) 

25 J 

The superscript (#) denotes a generalized inverse of the 
respective matrix that satisfies 0*0=1, such as the class of 
weighted pseudoinverses. The first condition requires that G 
have full column rank. Hence, this solution is only applicable 
30 to the hill-pose control case. Given full-pose control, one may 
make use of the fact that G=G and J=J. A may be introduced 
as the end-effector space inertia, where A _1= =JM _1 J r . These 
results can be interpreted as the active inertias that match the 
passive inertia. In other words, maintaining the natural inertia 
of the system eliminates the need for force feedback. 

3 5 It turns out that these two conditions do not account for the 

internal force components on the object. Thus, a third condi- 
tion is introduced to set the internal forces to zero. For the 
internal space, one may use a pseudoinverse for G r that is 
weighted by A -1 . That weighted pseudoinverse and its corre- 
40 sponding null space projection matrix are defined as follows: 

G^i 1 * =AG(G T AG)~ l 

N ( f=I-G A -\ r *G T (41) 

45 This weighted pseudoinverse keeps the object motion from 
disturbing the internal space. The third condition thus 
becomes: N G r f=0. Due to this condition, this control law is 
only applicable to rigid grasps, since they do not require 
internal forces to maintain grip. Accordingly, we will set 
50 G T# =G A -i r+ in equation (39). 

Applying these three conditions to equation (35), one may 
derive a zero force feedback control law: 


55 T z ff =-MJ+A-'G t a + _ 1 (F‘ +B 0 y + K 0 Ay+mg)+ (42 ) 

MJ*{h - 7?) - MNjM~ l (Bjq + KjAq) + c 

, n This expression was simplified by noting that G^-i + A -1 
G a -‘ t+ =A~ 1 G a -i t+ 

A closed-loop analysis of this control law reveals two inde- 
pendent dynamic relations for the object, the first in the exter- 
nal space and the second in the internal space. 


65 

(G T AG)y+BJ+K a Ay=AF 

(43) 


N c J'(AG)y=N G rf 

(44) 
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The first relation reveals the desired object impedance in 
equation (1), given an inertia that matches the passive inertia. 

For the second relation, it can be shown that N gj (AG)= 0 due 
to tlie weighted pseudoinverse. Fience, the weighted pseudo- 
inverse filters out the object accelerations from the internal 5 
space and in turn produces zero internal forces on the object 
20 . 

While the best modes for carrying out the invention have 
been described in detail, those familiar with the art to which 
this invention relates will recognize various alternative to 
designs and embodiments for practicing the invention within 
the scope of the appended claims. 

The invention claimed is: 

1. A robotic system comprising: 

a robot having a plurality of manipulators collectively 15 
adapted for grasping an object using one of a plurality of 
grasp types during an execution of a primary task having 
a task space and a null-space, wherein the manipulators 
have redundant degrees of freedom when grasping the 
object and the object has a predetermined number of 20 
degrees of freedom; and 

a controller that is electrically connected to the robot, and 
adapted to control the plurality of manipulators during 
the execution of the primary task using a multiple-task 
control hierarchy that includes a secondary task; 25 

wherein the controller: 

automatically parameterizes internal forces of the 
robotic system for each of the plurality grasp types in 
response to an input signal; 

defines the task space of the primary task at an object- 30 
level of control as a subset of the predetermined num- 
ber of degrees of freedom of the object, such that at 
least some of the predetermined number of degrees of 
freedom of the object are not commanded for the 
primary task; and 35 

incorporates the degrees of freedom that are not com- 
manded for the primary task into the null-space of the 
primary task to thereby increase a task space of the 
secondary task, wherein the task space of the second- 
ary task also includes at least some of the redundant 40 
degrees of freedom of the manipulators. 

2 . The robotic system of claim 1 , wherein the robot is a 
humanoid robot having at least 42 degrees of freedom. 

3. The robotic system of claim 1, wherein definition of the 
primary task at the object-level of control includes using at 45 
least one of a “closed-chain” Jacobian transformation and a 
“closed-chain” grasp matrix. 

4 . The robotic system of claim 1 , wherein the multiple-task 

control hierarchy utilizes an impedance relationship that 
operates in the null-space of the object-level of control. 50 

5 . The robotic system of claim 1 , wherein the controller is 
adapted to control only a subset of all available degrees of 
freedom (DOF) of the object using at least some of the plu- 
rality of manipulators in a cooperative grasp of the robot. 

6. A controller for a robotic system, the robotic system 55 
including at least one robot each having at least one manipu- 
lator adapted for grasping an object during execution of a 
primary task having a task space and a null-space, wherein the 

at least one manipulator has redundant degrees of freedom 
when grasping the object and the object has a predetermined 60 
number of degrees of freedom, the controller comprising: 

a host machine electrically connected to the at least one 
robot; and 

memory on which is stored an algorithm that is executable 
by the host machine, where execution of the algorithm 65 
by the host machine causes the host machine to control 
the at least one manipulator of the at least one robot 
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using a multiple-task control hierarchy that includes the 
primary task and a secondary task; 

wherein execution of the algorithm further causes the host 
machine to: 

automatically parameterize internal forces of the robotic 
system for each of a plurality of grasp types of the at 
least one robot in response to an input signal; define 
the task space of the primary task at an object-level of 
control by commanding a subset of the predetermined 
number of degrees of freedom of the object, such that 
at least some of the degrees of freedom of the object 
are not commanded for the primary task; and 
incorporate the degrees of freedom of the object that are 
not commanded into the null-space of the primary 
task to thereby increase a task space of the secondary 
task; and 

wherein the task space of the secondary task also includes 
at least one of the redundant degrees of freedom of the 
manipulators. 

7. The controller of claim 6, wherein the at least one robot 
includes a humanoid robot having at least 42 degrees of 
freedom. 

8. The controller of claim 6, wherein definition of the 
primary task at the object -level of control uses at least one of: 
a “closed-chain” Jacobian transformation and a “closed- 
chain” grasp matrix. 

9. A method for controlling a robotic system, the robotic 
system having a robot with a plurality of manipulators col- 
lectively adapted for grasping an object using one of a plu- 
rality of grasp types during execution of a primary taskhaving 
a task space and a null-space, wherein the plurality of 
manipulators has redundant degrees of freedom when grasp- 
ing the object and the object has a predetermined number of 
degrees of freedom, and a controller electrically connected to 
the robot, the controller being adapted to control the plurality 
of manipulators during the execution of the primary task, the 
method comprising: 

receiving an input signal via a host machine of the control- 
ler; 

processing the input signal via a multiple-task control hier- 
archy that includes the primary task and a secondary 
task, using the host machine, to thereby control the plu- 
rality of manipulators during the execution of the pri- 
mary task; 

wherein processing the input signal includes: 

defining the task space of the primary task at the object- 
level of control by commanding a subset of the pre- 
determined number of degrees of freedom of the 
object, such that at least some of the predetermined 
number of degrees of freedom of the object are not 
commanded for the primary task; 
automatically parameterizing internal forces of the 
robotic system for each of the plurality of grasp types 
in response to the input signal; and 
incorporating the degrees of freedom of the object that 
are not commanded into the null-space of the primary 
task to thereby increase a task space of the secondary 
task, wherein the task space of the secondary task also 
includes at least one of the redundant degrees of free- 
dom of the manipulators. 

10. The method of claim 9, wherein the plurality of grasp 
types includes a cooperative grasp type. 

11. The method of claim 9, wherein defining the primary 
task includes using at least one of: a “closed-chain” Jacobian 
transformation and a “closed-chain” grasp matrix. 





